#include <math.h>
#include "geo_std.h"
// #include <iostream>

// #define PI 3.141592653
// #define sign(x) ((x>=0) ? 1.0 : -1.0)

void v_ass(VECTOR v1,VECTOR v2)
{   v2[0]=v1[0]; v2[1]=v1[1]; v2[2]=v1[2];}
void v_add(VECTOR v1,VECTOR v2,VECTOR v3)
{   v3[0]=v1[0]+v2[0]; v3[1]=v1[1]+v2[1]; v3[2]=v1[2]+v2[2];}
void v_sub(VECTOR v1,VECTOR v2,VECTOR v3)
{   v3[0]=v1[0]-v2[0]; v3[1]=v1[1]-v2[1]; v3[2]=v1[2]-v2[2];}
REAL v_dot(VECTOR v1,VECTOR v2)
{  return(v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]); }
void v_cross(VECTOR v1,VECTOR v2,VECTOR v3) {
  VECTOR vtemp;
  vtemp[0] = v1[1]*v2[2] - v1[2]*v2[1];
  vtemp[1] = v1[2]*v2[0] - v1[0]*v2[2];
  vtemp[2] = v1[0]*v2[1] - v1[1]*v2[0];
  v3[0] = vtemp[0]; v3[1] = vtemp[1]; v3[2] = vtemp[2];
}
void sv_mul(REAL sc, VECTOR v1,VECTOR v2) {   
  v2[0]=sc*v1[0]; v2[1]=sc*v1[1]; v2[2]=sc*v1[2];
}
void m_ass(MATRIX m1,MATRIX m2) {
  int i,j;
  for(i=0;i<3;i++)
    for(j=0;j<3;j++)
      m2[i][j] = m1[i][j];
}
void m_mul(MATRIX m1,MATRIX m2,MATRIX m3) {
  int i,j;
  MATRIX mtemp;
  for(i=0;i<3;i++)
    for(j=0;j<3;j++)
      mtemp[i][j]=m1[i][0]*m2[0][j]+m1[i][1]*m2[1][j]+m1[i][2]*m2[2][j];
  for(i=0;i<3;i++) for(j=0;j<3;j++) m3[i][j] = mtemp[i][j];
}
void sm_mul(REAL sc, MATRIX m1, MATRIX m2) {
  int i,j;
  for(i=0;i<3;i++)
    for(j=0;j<3;j++)
      m2[i][j] = sc * m1[i][j];
}
void m_trans(MATRIX m1,MATRIX m2) {
  REAL temp;
  m2[0][0] = m1[0][0]; m2[1][1] = m1[1][1]; m2[2][2] = m1[2][2];
  temp = m1[0][1]; m2[0][1] = m1[1][0]; m2[1][0] = temp;
  temp = m1[1][2]; m2[1][2] = m1[2][1]; m2[2][1] = temp;
  temp = m1[2][0]; m2[2][0] = m1[0][2]; m2[0][2] = temp;
}
/*
S_MATRIX *m_inv(m1,m2)
S_MATRIX m1,m2;
{
	return(m2);
}
*/
void mv_mul(MATRIX m1, VECTOR v1,VECTOR v2) {
  VECTOR vtemp;
  int i;
  for(i=0;i<3;i++)
    vtemp[i] = m1[i][0]*v1[0] + m1[i][1]*v1[1] + m1[i][2]*v1[2];
  for(i=0;i<3;i++) v2[i]=vtemp[i];
}
void m_vec(int cl, MATRIX mx, VECTOR vec) {
  int i;
  for(i=0;i<3;i++)
    vec[i] = mx[i][cl];
}
void make_rot(REAL ang, VECTOR axis, MATRIX mtr) {
  REAL len,co,si;
  VECTOR temp;
  int i,j;
  len = sqrt(v_dot(axis,axis));
  if(len == 0.0 || ang == 0.0) {
    for(i=0;i<3;i++) {
      for(j=0;j<3;j++) mtr[i][j] = 0.0;
      mtr[i][i] = 1.0;
    }
  }
  else {
    co = cos(ang); si = sin(ang);
    for(i=0;i<3;i++) temp[i] = axis[i]/len;
    mtr[0][0] = temp[0]*temp[0]*(1.0 - co)+co;
    mtr[1][0] = temp[0]*temp[1]*(1.0 - co)+temp[2]*si;
    mtr[2][0] = temp[0]*temp[2]*(1.0 - co)-temp[1]*si;
    mtr[0][1] = temp[1]*temp[0]*(1.0 - co)-temp[2]*si;
    mtr[1][1] = temp[1]*temp[1]*(1.0 - co)+co;
    mtr[2][1] = temp[1]*temp[2]*(1.0 - co)+temp[0]*si;
    mtr[0][2] = temp[2]*temp[0]*(1.0 - co)+temp[1]*si;
    mtr[1][2] = temp[2]*temp[1]*(1.0 - co)-temp[0]*si;
    mtr[2][2] = temp[2]*temp[2]*(1.0 - co)+co;
  }
}
REAL rot_axis(MATRIX mtr, VECTOR axis) {
  REAL angl;
  REAL co,si,an;
  int i;
  co=(mtr[0][0]+mtr[1][1]+mtr[2][2]-1.0)/2.0;
  if (co <= -1.0) {
    //		printf("sqrt 1. co = %f\n",co);
    angl = PI;
    if(mtr[0][0]<=-1.0) {
      axis[0]=0.0;
    } else {
      axis[0] = sqrt((mtr[0][0] + 1.0)/2.0);
    }
    if(mtr[1][1]<=-1.0) {
      axis[1]=0.0;
    } else {
      axis[1] = sign(mtr[0][1])*sqrt((mtr[1][1]+1.0)/2.0);
    }
    if(mtr[2][2]<=-1.0) {
      axis[2]=0.0;
    } else {
      axis[2] = sign(mtr[0][2])*sqrt((mtr[2][2]+1.0)/2.0);
    }
  } else if(co < 1.0) {
    axis[0] = mtr[2][1] - mtr[1][2];
    axis[1] = mtr[0][2] - mtr[2][0];
    axis[2] = mtr[1][0] - mtr[0][1];
    an = sqrt(v_dot(axis,axis));
    if(an == 0.0) {
      //			printf("sqrt 2\n");
      // for(i=0;i<3;i++) axis[i] = sqrt((mtr[i][i]-co)/(1.0 - co));
      angl = 0.0;
      axis[0] = 1.0;
      axis[1] = axis[2] = 0;
      co = 1.0;
      si = 0.0;
    } else {
      for(i=0;i<3;i++) axis[i] = axis[i]/an;
      si = an/2.0;
      angl = arctan2(si,co);
    }
  } else {
    angl = 0.0;
    axis[0] = 1.0;
    axis[1] = axis[2] = 0;
  }
  return(angl);
}
REAL arctan2(REAL y,REAL x) {
  REAL ang;
  if(x>0.0) ang = atan(y/x);
  else if(x<0.0) {
    if(y>0.0) ang = atan(y/x) + PI;
    else if (y < 0.0) ang = atan(y/x) - PI;
    else ang = PI;
  }
  else if(y > 0.0) ang = PI/2.0;
  else if(y < 0.0) ang = - PI/2.0;
  else ang = 0.0;
  return(ang);
}
void make_frame(MATRIX fig, VECTOR pos, FRAME& frm) {
  int i,j;
  for(i=0;i<3;i++) {
    for(j=0;j<3;j++) frm.mtr[i][j]=fig[i][j];
    frm.vtr[i] = pos[i];
  }
}
void f_mul(FRAME& f1,FRAME& f2,FRAME& f3) {
  VECTOR vtemp;
  mv_mul(f1.mtr,f2.vtr,vtemp);
  v_add(f1.vtr,vtemp,f3.vtr);
  m_mul(f1.mtr,f2.mtr,f3.mtr);
}
void f_inv(FRAME& f1,FRAME& f2) {
  f2.vtr[0] = -f1.vtr[0];
  f2.vtr[1] = -f1.vtr[1];
  f2.vtr[2] = -f1.vtr[2];
  m_trans(f1.mtr,f2.mtr);
  mv_mul(f2.mtr,f2.vtr,f2.vtr);
}
void f_ass(FRAME& f1,FRAME& f2) {
  v_ass(f1.vtr,f2.vtr);
  m_ass(f1.mtr,f2.mtr);
}
//!
int ma(REAL a, MATRIX mx) {
  int i,j;
  for(i=0; i<3; i++)
    for(j=0; j<3; j++) 
      mx[i][j]=0.0;
  mx[0][0] = 1.0;
  mx[1][1] = cos(a);
  mx[2][2] = cos(a);
  mx[1][2] = -sin(a);
  mx[2][1] = sin(a);
  return 0;
}
//!
int mb(REAL a, MATRIX mx) {
  int i,j;
  for(i=0; i<3; i++)
    for(j=0; j<3; j++) 
      mx[i][j]=0.0;
  mx[1][1] = 1.0;
  mx[2][2] = cos(a);
  mx[0][0] = cos(a);
  mx[2][0] = -sin(a);
  mx[0][2] = sin(a);
  return 0;
}
//!
int mc(REAL a, MATRIX mx) {
  int i,j;
  for(i=0; i<3; i++)
    for(j=0; j<3; j++) 
      mx[i][j]=0.0;
  mx[2][2] = 1.0;
  mx[0][0] = cos(a);
  mx[1][1] = cos(a);
  mx[0][1] = -sin(a);
  mx[1][0] = sin(a);
  return 0;
}
int mabc(MATRIX mx, REAL abc[3]) {
  if (mx[0][2]> 1.0) {
    abc[1] = PI/2.0;
    abc[0] = 0.0;
    abc[2] = arctan2(mx[2][1],mx[1][1]);
  } else if (mx[0][2] < -1.0) {
    abc[1] = - PI/2.0;
    abc[0] = 0.0;
    abc[2] = arctan2(mx[1][0],mx[2][0]);
  } else {
    abc[1] = asin(mx[0][2]);
    abc[0]=arctan2(-mx[1][2],mx[2][2]);
    abc[2]=arctan2(-mx[0][1],mx[0][0]);
  }
  return 0;
}
